#include "i2cmaster.h"
#include "TMC_Commands.h"

#define DEV_TMC222_1	0xC0  	//HW-Bit on GND
#define DEV_TMC222_2	0xC2		//HW-Bit on VCC
#define DEV_BROADCAST	0x00




void main(){

	TTMC222Status DEV_TMC222_1_fullStatus1;
	int i;
	unsigned char* pPosition = DEV_TMC222_1_fullStatus1;

	for(i=0;i<6;i++){
		*(pPosition+i) = 0x00;
	 }
	
	i2c_init();
	
	ret = getFullStatus1(DEV_TMC222_1, DEV_TMC222_1_fullStatus1);
	setMotorParam(DEV_TMC222_1);
	resetPosition(DEV_TMC222_1);
	runInit(DEV_TMC222_1);
	
	
	
}

void GetFullStatus1(unsigned char addr, TTMC222Status *TMC222Status){
	int i;
	unsigned char pPosition = &TMC222Status;
	unsigned char ack;
	
	//Trying to get writing access to first TMC222-Device
	ack = i2c_start(addr + I2C_WRITE);
	if(!ack){
		i2c_write(GETFULLSTATUS1);
		i2c_stop();
		//Trying to get read access
		ack = i2c_rep_start(addr + I2C_READ);
		if(!ack){
			for(i=0;i<5;i++){
				*(pPosition+i) = i2c_readAck();
			}
			*(pPosition+5) = i2c_readNak();
			i2c_stop();
		}else{/*Was not able to access device*/
			i2c_stop();		
		}
	}else{/*Was not able to access device*/
		i2c_stop();
	}
}

void setMotorParam(unsigned char addr){//Irun, Ihold, Vmax, Vmin usw???
	unsigned char ack;
	ack = i2c_start_wait(addr + I2C_WRITE);
	if(!ack){
		i2c_write(SETMOTORPARAM);
		i2c_write(0xFF);
		i2c_write(0xFF);
		i2c_write(0xf5);						// Byte 4 IRUN/IHOLD
		i2c_write(0x10);						// Byte 5 VMAX/VMIN
		i2c_write(0x04);						// Status, ...
		i2c_write(0x01);						// Byte 7 SecurePos (7-0)
		i2c_write(0x03);

		i2c_stop();
		//Trying to get read access
	}else{/*Was not able to access device*/
		i2c_stop();
	}
}

void runInit(unsigned char addr){
	unsigned char ack;
	ack = i2c_start_wait(addr + I2C_WRITE);
	if(!ack){
		i2c_write(RUNINIT);
		i2c_write(0xFF);
		i2c_write(0xFF);
		i2c_write(0x10);						// Byte 5 VMAX/VMIN
		i2c_write(0x7f);						// Byte 5 Byte 1 Highbyte
		i2c_write(0xff);						// Byte 6 Byte 0 Lowbyte
		i2c_write(0x80);						// Byte 7 Byte 1 Highbyte
		i2c_write(0x05);						// Byte 8 Byte 0 Lowbyte
		i2c_stop();
		
	}else{/*Was not able to access device*/
		i2c_stop();
	}
}

void resetPosition(unsigned char addr){
	unsigned char ack;
	ack = i2c_start_wait(addr + I2C_WRITE);
	if(!ack){
		i2c_write(RESETPOSITION);
		i2c_stop();

	}else{/*Was not able to access device*/
		i2c_stop();
	}
}
